#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>

#include <libplayerc/playerc.h>

#define NUM_ROBOTS 3 

typedef struct {
	int x;
	int y;
} point;

point fires[3] = {{125, 67}, {174, 180}, {223, 76}};

int main(int argc, char **argv) {
	int i;
	int j;
	int sweep;
	int fires_exist = sizeof(fires)/sizeof(point);
	playerc_client_t *client[NUM_ROBOTS];
	playerc_position2d_t *position[NUM_ROBOTS];
	playerc_laser_t *laser[NUM_ROBOTS];

	for(i=0; i<NUM_ROBOTS; i++) {
		client[i] = playerc_client_create(NULL, "localhost", 6665);
		if(playerc_client_connect(client[i]) != 0) {
			printf("Unable to connect...\n");
			return -1;
		}

		position[i] = playerc_position2d_create(client[i], i);
		playerc_position2d_subscribe(position[i], PLAYER_OPEN_MODE);

		laser[i] = playerc_laser_create(client[i], i);
		playerc_laser_subscribe(laser[i], PLAYER_OPEN_MODE);
	}

	for(i=0; i<NUM_ROBOTS; i++) {
			playerc_client_read(client[i]);
			playerc_client_read(client[i]);
	}

	while(fires_exist) {
		// Check if robots have been successful
		for(i=0; i<fires_exist; i++)
			if(fires[i].x && fires[i].y)
				break;
		if(i>fires_exist)
			fires_exist=0;

		// Read robot sensors
		for(i=0; i<NUM_ROBOTS; i++) {
			playerc_client_read(client[i]);
		}
		// Fire sensor
		for(i=0; i<NUM_ROBOTS; i++) {
			for(i=0; i<fires_exist; i++) {
				
			}
		}

		//printf("r0p0: %f %f\n", position[0]->px, position[0]->py);

		for(j=0; j<NUM_ROBOTS; j++) {
			playerc_position2d_set_cmd_vel(position[j], 0.5, 0, 0, 1);
			sweep = (laser[j]->scan_count)/2;
			for(i=sweep-sweep/2; i<sweep+sweep/2; i++) {
				if(laser[j]->ranges[i])
				if(laser[j]->ranges[i] < 0.9) {
					playerc_position2d_set_cmd_vel(position[j], 0, 0, DTOR(30+30*rand()%2), 1);
					break;
				} else if(laser[j]->ranges[i] < 1.6) {
					playerc_position2d_set_cmd_vel(position[j], 0.3, 0, DTOR(-15-15*rand()%2), 1);
				}
			}
		}

	}

	return 0;
}

